#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <iostream>
#include <string>
#include <sstream>
#include <std_msgs/Int16.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Int8.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Empty.h>
#include <stdlib.h>
#include <cstdio>
#include <cmath>
#include <tof_serial_pkg/TOF_SENSE_DATA.h>
#include <numeric>

// 函数功能：将数据经由串口发送出去
// 入口参数1：[serial::Serial &ser]：     串口类名称
// 入口参数2：[std::string &serial_msg]:  要通过串口发送出去的字符串
int serial_write(serial::Serial &ser, std::string &serial_msg)
{
    ser.write(serial_msg);
    return 0;
}

// 函数功能：将从串口接收到的数据保存到数组中
// 入口参数1：[serial::Serial &ser]：     串口类名称
// 入口参数2：[std::string &serial_msg]:  从串口读取的字符串
int serial_read(serial::Serial &ser, std::string &serial_msg)
{
    serial_msg = ser.read(ser.available());
    return 0;
}

int main(int argc, char **argv)
{

    setlocale(LC_CTYPE, "zh_CN.utf8"); // 设置编码，防止中文乱码

    // 初始化，节点名为serial_publisher
    ros::init(argc, argv, "serial_publisher");
    // 创建句柄seuNB，用于管理资源
    ros::NodeHandle nh;
    // 裸数据
    ros::Publisher tof_data_pub = nh.advertise<tof_serial_pkg::TOF_SENSE_DATA>("tof_sense_data", 50);
    tof_serial_pkg::TOF_SENSE_DATA tof_sense_data;

    // 实例化一个serial类
    serial::Serial ser2;
    // 初始化串口相关设置
    ser2.setPort("/dev/ttyACM0");                              // 设置打开的串口名称:这里打开一个虚拟串口
    ser2.setBaudrate(921600);                                  // 设置串口的波特率
    serial::Timeout to = serial::Timeout::simpleTimeout(1000); // 创建timeout
    ser2.setTimeout(to);                                       // 设置串口的timeout

    // 打开串口
    try
    {
        ser2.open(); // 打开串口
    }

    catch (const std::exception &e)
    {
        ROS_ERROR_STREAM("Unable to open port."); // 打开串口失败，打印日志信息，然后结束程序
        return -1;
    }

    // 判断串口是否成功打开
    if (ser2.isOpen())
    {
        ROS_INFO_STREAM("Serial Port is opened.\n"); // 成功打开串口，打印日志信息
    }
    else
    {
        return -1; // 打开串口失败，打印日志信息，然后结束程序
    }

    ros::Rate loop_rate(15); // 指定循环频率50
    while (ros::ok())
    {
        // 将数据传入到serial_msg1中
        // 获取缓冲区内的字节数
        size_t n = ser2.available();
        uint8_t buffer[1024];
        if (n != 0)
        {
            uint8_t buffer[1024];
            n = ser2.read(buffer, n);
            unsigned int crcCode;
            uint8_t header[1024] = {
                buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5], buffer[6], buffer[7], buffer[8],
                buffer[9], buffer[10], buffer[11], buffer[12], buffer[13], buffer[14],        // 1
                buffer[15], buffer[16], buffer[17], buffer[18], buffer[19], buffer[20],       // 2
                buffer[21], buffer[22], buffer[23], buffer[24], buffer[25], buffer[26],       // 3
                buffer[27], buffer[28], buffer[29], buffer[30], buffer[31], buffer[32],       // 4
                buffer[33], buffer[34], buffer[35], buffer[36], buffer[37], buffer[38],       // 5
                buffer[39], buffer[40], buffer[41], buffer[42], buffer[43], buffer[44],       // 6
                buffer[45], buffer[46], buffer[47], buffer[48], buffer[49], buffer[50],       // 7
                buffer[51], buffer[52], buffer[53], buffer[54], buffer[55], buffer[56],       // 8
                buffer[57], buffer[58], buffer[59], buffer[60], buffer[61], buffer[62],       // 9
                buffer[63], buffer[64], buffer[65], buffer[66], buffer[67], buffer[68],       // 10
                buffer[69], buffer[70], buffer[71], buffer[72], buffer[73], buffer[74],       // 11
                buffer[75], buffer[76], buffer[77], buffer[78], buffer[79], buffer[80],       // 12
                buffer[81], buffer[82], buffer[83], buffer[84], buffer[85], buffer[86],       // 13
                buffer[87], buffer[88], buffer[89], buffer[90], buffer[91], buffer[92],       // 14
                buffer[93], buffer[94], buffer[95], buffer[96], buffer[97], buffer[98],       // 15
                buffer[99], buffer[100], buffer[101], buffer[102], buffer[103], buffer[104],  // 16
                buffer[105], buffer[106], buffer[107], buffer[108], buffer[109], buffer[110], // 17
                buffer[111], buffer[112], buffer[113], buffer[114], buffer[115], buffer[116], // 18
                buffer[117], buffer[118], buffer[119], buffer[120], buffer[121], buffer[122], // 19
                buffer[123], buffer[124], buffer[125], buffer[126], buffer[127], buffer[128], // 20
                buffer[129], buffer[130], buffer[131], buffer[132], buffer[133], buffer[134], // 21
                buffer[135], buffer[136], buffer[137], buffer[138], buffer[139], buffer[140], // 22
                buffer[141], buffer[142], buffer[143], buffer[144], buffer[145], buffer[146], // 23
                buffer[147], buffer[148], buffer[149], buffer[150], buffer[151], buffer[152], // 24
                buffer[153], buffer[154], buffer[155], buffer[156], buffer[157], buffer[158], // 25
                buffer[159], buffer[160], buffer[161], buffer[162], buffer[163], buffer[164], // 26
                buffer[165], buffer[166], buffer[167], buffer[168], buffer[169], buffer[170], // 27
                buffer[171], buffer[172], buffer[173], buffer[174], buffer[175], buffer[176], // 28
                buffer[177], buffer[178], buffer[179], buffer[180], buffer[181], buffer[182], // 29
                buffer[183], buffer[184], buffer[185], buffer[186], buffer[187], buffer[188], // 30
                buffer[189], buffer[190], buffer[191], buffer[192], buffer[193], buffer[194], // 31
                buffer[195], buffer[196], buffer[197], buffer[198], buffer[199], buffer[200], // 32
                buffer[201], buffer[202], buffer[203], buffer[204], buffer[205], buffer[206], // 33
                buffer[207], buffer[208], buffer[209], buffer[210], buffer[211], buffer[212], // 34
                buffer[213], buffer[214], buffer[215], buffer[216], buffer[217], buffer[218], // 35
                buffer[219], buffer[220], buffer[221], buffer[222], buffer[223], buffer[224], // 36
                buffer[225], buffer[226], buffer[227], buffer[228], buffer[229], buffer[230], // 37
                buffer[231], buffer[232], buffer[233], buffer[234], buffer[235], buffer[236], // 38
                buffer[237], buffer[238], buffer[239], buffer[240], buffer[241], buffer[242], // 39
                buffer[243], buffer[244], buffer[245], buffer[246], buffer[247], buffer[248], // 40
                buffer[249], buffer[250], buffer[251], buffer[252], buffer[253], buffer[254], // 41
                buffer[255], buffer[256], buffer[257], buffer[258], buffer[259], buffer[260], // 42
                buffer[261], buffer[262], buffer[263], buffer[264], buffer[265], buffer[266], // 43
                buffer[267], buffer[268], buffer[269], buffer[270], buffer[271], buffer[272], // 44
                buffer[273], buffer[274], buffer[275], buffer[276], buffer[277], buffer[278], // 45
                buffer[279], buffer[280], buffer[281], buffer[282], buffer[283], buffer[284], // 46
                buffer[285], buffer[286], buffer[287], buffer[288], buffer[289], buffer[290], // 47
                buffer[291], buffer[292], buffer[293], buffer[294], buffer[295], buffer[296], // 48
                buffer[297], buffer[298], buffer[299], buffer[300], buffer[301], buffer[302], // 49
                buffer[303], buffer[304], buffer[305], buffer[306], buffer[307], buffer[308], // 50
                buffer[309], buffer[310], buffer[311], buffer[312], buffer[313], buffer[314], // 51
                buffer[315], buffer[316], buffer[317], buffer[318], buffer[319], buffer[320], // 52
                buffer[321], buffer[322], buffer[323], buffer[324], buffer[325], buffer[326], // 53
                buffer[327], buffer[328], buffer[329], buffer[330], buffer[331], buffer[332], // 54
                buffer[333], buffer[334], buffer[335], buffer[336], buffer[337], buffer[338], // 55
                buffer[339], buffer[340], buffer[341], buffer[342], buffer[343], buffer[344], // 56
                buffer[345], buffer[346], buffer[347], buffer[348], buffer[349], buffer[350], // 57
                buffer[351], buffer[352], buffer[353], buffer[354], buffer[355], buffer[356], // 58
                buffer[357], buffer[358], buffer[359], buffer[360], buffer[361], buffer[362], // 59
                buffer[363], buffer[364], buffer[365], buffer[366], buffer[367], buffer[368], // 60
                buffer[369], buffer[370], buffer[371], buffer[372], buffer[373], buffer[374], // 61
                buffer[375], buffer[376], buffer[377], buffer[378], buffer[379], buffer[380], // 62
                buffer[381], buffer[382], buffer[383], buffer[384], buffer[385], buffer[386], // 63
                buffer[387], buffer[388], buffer[389], buffer[390], buffer[391], buffer[392], // 64
                buffer[393]};
            int head0 = buffer[0];
            if (head0 == 87)
            {
                int dis_0 = buffer[11] << 16 | buffer[10] << 8 | buffer[9];
                int dis_status_0 = buffer[12];
                int signal_0 = buffer[14] << 8 | buffer[13];

                int dis_1 = buffer[17] << 16 | buffer[16] << 8 | buffer[15];
                int dis_status_1 = buffer[18];
                int signal_1 = buffer[20] << 8 | buffer[19];

                int dis_2 = buffer[23] << 16 | buffer[22] << 8 | buffer[21];
                int dis_status_2 = buffer[24];
                int signal_2 = buffer[26] << 8 | buffer[25];

                int dis_3 = buffer[29] << 16 | buffer[28] << 8 | buffer[27];
                int dis_status_3 = buffer[30];
                int signal_3 = buffer[32] << 8 | buffer[31];

                int dis_4 = buffer[35] << 16 | buffer[34] << 8 | buffer[33];
                int dis_status_4 = buffer[36];
                int signal_4 = buffer[38] << 8 | buffer[37];

                int dis_5 = buffer[41] << 16 | buffer[40] << 8 | buffer[39];
                int dis_status_5 = buffer[42];
                int signal_5 = buffer[44] << 8 | buffer[43];

                int dis_6 = buffer[47] << 16 | buffer[46] << 8 | buffer[45];
                int dis_status_6 = buffer[48];
                int signal_6 = buffer[50] << 8 | buffer[49];

                int dis_7 = buffer[53] << 16 | buffer[52] << 8 | buffer[51];
                int dis_status_7 = buffer[54];
                int signal_7 = buffer[56] << 8 | buffer[55];

                int dis_8 = buffer[59] << 16 | buffer[58] << 8 | buffer[57];
                int dis_status_8 = buffer[60];
                int signal_8 = buffer[62] << 8 | buffer[61];

                int dis_9 = buffer[65] << 16 | buffer[64] << 8 | buffer[63];
                int dis_status_9 = buffer[66];
                int signal_9 = buffer[68] << 8 | buffer[67];

                int dis_10 = buffer[71] << 16 | buffer[70] << 8 | buffer[69];
                int dis_status_10 = buffer[72];
                int signal_10 = buffer[74] << 8 | buffer[73];

                int dis_11 = buffer[77] << 16 | buffer[76] << 8 | buffer[75];
                int dis_status_11 = buffer[78];
                int signal_11 = buffer[80] << 8 | buffer[79];

                int dis_12 = buffer[83] << 16 | buffer[82] << 8 | buffer[81];
                int dis_status_12 = buffer[84];
                int signal_12 = buffer[86] << 8 | buffer[85];

                int dis_13 = buffer[89] << 16 | buffer[88] << 8 | buffer[87];
                int dis_status_13 = buffer[90];
                int signal_13 = buffer[92] << 8 | buffer[91];

                int dis_14 = buffer[95] << 16 | buffer[94] << 8 | buffer[93];
                int dis_status_14 = buffer[96];
                int signal_14 = buffer[98] << 8 | buffer[97];

                int dis_15 = buffer[101] << 16 | buffer[100] << 8 | buffer[99];
                int dis_status_15 = buffer[102];
                int signal_15 = buffer[104] << 8 | buffer[103];

                int dis_16 = buffer[107] << 16 | buffer[106] << 8 | buffer[105];
                int dis_status_16 = buffer[108];
                int signal_16 = buffer[110] << 8 | buffer[109];

                int dis_17 = buffer[113] << 16 | buffer[112] << 8 | buffer[111];
                int dis_status_17 = buffer[114];
                int signal_17 = buffer[116] << 8 | buffer[115];

                int dis_18 = buffer[119] << 16 | buffer[118] << 8 | buffer[117];
                int dis_status_18 = buffer[120];
                int signal_18 = buffer[122] << 8 | buffer[121];

                int dis_19 = buffer[125] << 16 | buffer[124] << 8 | buffer[123];
                int dis_status_19 = buffer[126];
                int signal_19 = buffer[128] << 8 | buffer[127];

                int dis_20 = buffer[131] << 16 | buffer[130] << 8 | buffer[129];
                int dis_status_20 = buffer[132];
                int signal_20 = buffer[134] << 8 | buffer[133];

                int dis_21 = buffer[137] << 16 | buffer[136] << 8 | buffer[135];
                int dis_status_21 = buffer[138];
                int signal_21 = buffer[140] << 8 | buffer[139];

                int dis_22 = buffer[143] << 16 | buffer[142] << 8 | buffer[141];
                int dis_status_22 = buffer[144];
                int signal_22 = buffer[146] << 8 | buffer[145];

                int dis_23 = buffer[149] << 16 | buffer[148] << 8 | buffer[147];
                int dis_status_23 = buffer[150];
                int signal_23 = buffer[152] << 8 | buffer[151];

                int dis_24 = buffer[155] << 16 | buffer[154] << 8 | buffer[153];
                int dis_status_24 = buffer[156];
                int signal_24 = buffer[158] << 8 | buffer[157];

                int dis_25 = buffer[161] << 16 | buffer[160] << 8 | buffer[159];
                int dis_status_25 = buffer[162];
                int signal_25 = buffer[164] << 8 | buffer[163];

                int dis_26 = buffer[167] << 16 | buffer[166] << 8 | buffer[165];
                int dis_status_26 = buffer[168];
                int signal_26 = buffer[170] << 8 | buffer[169];

                int dis_27 = buffer[173] << 16 | buffer[172] << 8 | buffer[171];
                int dis_status_27 = buffer[174];
                int signal_27 = buffer[176] << 8 | buffer[175];

                int dis_28 = buffer[179] << 16 | buffer[178] << 8 | buffer[177];
                int dis_status_28 = buffer[180];
                int signal_28 = buffer[182] << 8 | buffer[181];

                int dis_29 = buffer[185] << 16 | buffer[184] << 8 | buffer[183];
                int dis_status_29 = buffer[186];
                int signal_29 = buffer[188] << 8 | buffer[187];

                int dis_30 = buffer[191] << 16 | buffer[190] << 8 | buffer[189];
                int dis_status_30 = buffer[192];
                int signal_30 = buffer[194] << 8 | buffer[193];

                int dis_31 = buffer[197] << 16 | buffer[196] << 8 | buffer[195];
                int dis_status_31 = buffer[198];
                int signal_31 = buffer[200] << 8 | buffer[199];

                int dis_32 = buffer[203] << 16 | buffer[202] << 8 | buffer[201];
                int dis_status_32 = buffer[204];
                int signal_32 = buffer[206] << 8 | buffer[205];

                int dis_33 = buffer[209] << 16 | buffer[208] << 8 | buffer[207];
                int dis_status_33 = buffer[210];
                int signal_33 = buffer[212] << 8 | buffer[211];

                int dis_34 = buffer[215] << 16 | buffer[214] << 8 | buffer[213];
                int dis_status_34 = buffer[216];
                int signal_34 = buffer[218] << 8 | buffer[217];

                int dis_35 = buffer[221] << 16 | buffer[220] << 8 | buffer[219];
                int dis_status_35 = buffer[222];
                int signal_35 = buffer[224] << 8 | buffer[223];

                int dis_36 = buffer[227] << 16 | buffer[226] << 8 | buffer[225];
                int dis_status_36 = buffer[228];
                int signal_36 = buffer[230] << 8 | buffer[229];

                int dis_37 = buffer[233] << 16 | buffer[232] << 8 | buffer[231];
                int dis_status_37 = buffer[234];
                int signal_37 = buffer[236] << 8 | buffer[235];

                int dis_38 = buffer[239] << 16 | buffer[238] << 8 | buffer[237];
                int dis_status_38 = buffer[240];
                int signal_38 = buffer[242] << 8 | buffer[241];

                int dis_39 = buffer[245] << 16 | buffer[244] << 8 | buffer[243];
                int dis_status_39 = buffer[246];
                int signal_39 = buffer[248] << 8 | buffer[247];

                int dis_40 = buffer[251] << 16 | buffer[250] << 8 | buffer[249];
                int dis_status_40 = buffer[252];
                int signal_40 = buffer[254] << 8 | buffer[193];

                int dis_41 = buffer[257] << 16 | buffer[256] << 8 | buffer[255];
                int dis_status_41 = buffer[258];
                int signal_41 = buffer[260] << 8 | buffer[259];

                int dis_42 = buffer[263] << 16 | buffer[262] << 8 | buffer[261];
                int dis_status_42 = buffer[264];
                int signal_42 = buffer[266] << 8 | buffer[265];

                int dis_43 = buffer[269] << 16 | buffer[268] << 8 | buffer[267];
                int dis_status_43 = buffer[270];
                int signal_43 = buffer[272] << 8 | buffer[271];

                int dis_44 = buffer[275] << 16 | buffer[274] << 8 | buffer[273];
                int dis_status_44 = buffer[276];
                int signal_44 = buffer[278] << 8 | buffer[277];

                int dis_45 = buffer[281] << 16 | buffer[280] << 8 | buffer[279];
                int dis_status_45 = buffer[282];
                int signal_45 = buffer[284] << 8 | buffer[283];

                int dis_46 = buffer[287] << 16 | buffer[286] << 8 | buffer[285];
                int dis_status_46 = buffer[288];
                int signal_46 = buffer[290] << 8 | buffer[289];

                int dis_47 = buffer[293] << 16 | buffer[292] << 8 | buffer[291];
                int dis_status_47 = buffer[294];
                int signal_47 = buffer[296] << 8 | buffer[295];

                int dis_48 = buffer[299] << 16 | buffer[298] << 8 | buffer[297];
                int dis_status_48 = buffer[300];
                int signal_48 = buffer[302] << 8 | buffer[301];

                int dis_49 = buffer[305] << 16 | buffer[304] << 8 | buffer[303];
                int dis_status_49 = buffer[306];
                int signal_49 = buffer[308] << 8 | buffer[307];

                int dis_50 = buffer[311] << 16 | buffer[310] << 8 | buffer[309];
                int dis_status_50 = buffer[312];
                int signal_50 = buffer[314] << 8 | buffer[313];

                int dis_51 = buffer[317] << 16 | buffer[316] << 8 | buffer[315];
                int dis_status_51 = buffer[318];
                int signal_51 = buffer[320] << 8 | buffer[319];

                int dis_52 = buffer[323] << 16 | buffer[322] << 8 | buffer[321];
                int dis_status_52 = buffer[324];
                int signal_52 = buffer[326] << 8 | buffer[325];

                int dis_53 = buffer[329] << 16 | buffer[328] << 8 | buffer[327];
                int dis_status_53 = buffer[330];
                int signal_53 = buffer[332] << 8 | buffer[331];

                int dis_54 = buffer[335] << 16 | buffer[334] << 8 | buffer[333];
                int dis_status_54 = buffer[336];
                int signal_54 = buffer[338] << 8 | buffer[337];

                int dis_55 = buffer[341] << 16 | buffer[340] << 8 | buffer[339];
                int dis_status_55 = buffer[342];
                int signal_55 = buffer[344] << 8 | buffer[343];

                int dis_56 = buffer[347] << 16 | buffer[346] << 8 | buffer[345];
                int dis_status_56 = buffer[348];
                int signal_56 = buffer[350] << 8 | buffer[349];

                int dis_57 = buffer[353] << 16 | buffer[352] << 8 | buffer[351];
                int dis_status_57 = buffer[354];
                int signal_57 = buffer[356] << 8 | buffer[355];

                int dis_58 = buffer[359] << 16 | buffer[358] << 8 | buffer[357];
                int dis_status_58 = buffer[360];
                int signal_58 = buffer[362] << 8 | buffer[361];

                int dis_59 = buffer[365] << 16 | buffer[364] << 8 | buffer[363];
                int dis_status_59 = buffer[366];
                int signal_59 = buffer[368] << 8 | buffer[367];

                int dis_60 = buffer[371] << 16 | buffer[370] << 8 | buffer[369];
                int dis_status_60 = buffer[372];
                int signal_60 = buffer[374] << 8 | buffer[373];

                int dis_61 = buffer[377] << 16 | buffer[376] << 8 | buffer[375];
                int dis_status_61 = buffer[378];
                int signal_61 = buffer[380] << 8 | buffer[379];

                int dis_62 = buffer[383] << 16 | buffer[382] << 8 | buffer[381];
                int dis_status_62 = buffer[384];
                int signal_62 = buffer[386] << 8 | buffer[385];

                int dis_63 = buffer[389] << 16 | buffer[388] << 8 | buffer[387];
                int dis_status_63 = buffer[390];
                int signal_63 = buffer[392] << 8 | buffer[391];

                tof_sense_data.dis = {
                    dis_0,
                    dis_1,
                    dis_2,
                    dis_3,
                    dis_4,
                    dis_5,
                    dis_6,
                    dis_7,
                    dis_8,
                    dis_9,
                    dis_10,
                    dis_11,
                    dis_12,
                    dis_13,
                    dis_14,
                    dis_15,
                    dis_16,
                    dis_17,
                    dis_18,
                    dis_19,
                    dis_20,
                    dis_21,
                    dis_22,
                    dis_23,
                    dis_24,
                    dis_25,
                    dis_26,
                    dis_27,
                    dis_28,
                    dis_29,
                    dis_30,
                    dis_31,
                    dis_32,
                    dis_33,
                    dis_34,
                    dis_35,
                    dis_36,
                    dis_37,
                    dis_38,
                    dis_39,
                    dis_40,
                    dis_41,
                    dis_42,
                    dis_43,
                    dis_44,
                    dis_45,
                    dis_46,
                    dis_47,
                    dis_48,
                    dis_49,
                    dis_50,
                    dis_51,
                    dis_52,
                    dis_53,
                    dis_54,
                    dis_55,
                    dis_56,
                    dis_57,
                    dis_58,
                    dis_59,
                    dis_60,
                    dis_61,
                    dis_62,
                    dis_63,
                };
                tof_sense_data.dis_status = {
                    dis_status_0,
                    dis_status_1,
                    dis_status_2,
                    dis_status_3,
                    dis_status_4,
                    dis_status_5,
                    dis_status_6,
                    dis_status_7,
                    dis_status_8,
                    dis_status_9,
                    dis_status_10,
                    dis_status_11,
                    dis_status_12,
                    dis_status_13,
                    dis_status_14,
                    dis_status_15,
                    dis_status_16,
                    dis_status_17,
                    dis_status_18,
                    dis_status_19,
                    dis_status_20,
                    dis_status_21,
                    dis_status_22,
                    dis_status_23,
                    dis_status_24,
                    dis_status_25,
                    dis_status_26,
                    dis_status_27,
                    dis_status_28,
                    dis_status_29,
                    dis_status_30,
                    dis_status_31,
                    dis_status_32,
                    dis_status_33,
                    dis_status_34,
                    dis_status_35,
                    dis_status_36,
                    dis_status_37,
                    dis_status_38,
                    dis_status_39,
                    dis_status_40,
                    dis_status_41,
                    dis_status_42,
                    dis_status_43,
                    dis_status_44,
                    dis_status_45,
                    dis_status_46,
                    dis_status_47,
                    dis_status_48,
                    dis_status_49,
                    dis_status_50,
                    dis_status_51,
                    dis_status_52,
                    dis_status_53,
                    dis_status_54,
                    dis_status_55,
                    dis_status_56,
                    dis_status_57,
                    dis_status_58,
                    dis_status_59,
                    dis_status_60,
                    dis_status_61,
                    dis_status_62,
                    dis_status_63,
                };
                tof_sense_data.signal = {
                    signal_0,
                    signal_1,
                    signal_2,
                    signal_3,
                    signal_4,
                    signal_5,
                    signal_6,
                    signal_7,
                    signal_8,
                    signal_9,
                    signal_10,
                    signal_11,
                    signal_12,
                    signal_13,
                    signal_14,
                    signal_15,
                    signal_16,
                    signal_17,
                    signal_18,
                    signal_19,
                    signal_20,
                    signal_21,
                    signal_22,
                    signal_23,
                    signal_24,
                    signal_25,
                    signal_26,
                    signal_27,
                    signal_28,
                    signal_29,
                    signal_30,
                    signal_31,
                    signal_32,
                    signal_33,
                    signal_34,
                    signal_35,
                    signal_36,
                    signal_37,
                    signal_38,
                    signal_39,
                    signal_40,
                    signal_41,
                    signal_42,
                    signal_43,
                    signal_44,
                    signal_45,
                    signal_46,
                    signal_47,
                    signal_48,
                    signal_49,
                    signal_50,
                    signal_51,
                    signal_52,
                    signal_53,
                    signal_54,
                    signal_55,
                    signal_56,
                    signal_57,
                    signal_58,
                    signal_59,
                    signal_60,
                    signal_61,
                    signal_62,
                    signal_63,
                };

                std::vector<int> dis_data_4x4 = {
                    dis_18,
                    dis_19,
                    dis_20,
                    dis_21,
                    dis_26,
                    dis_27,
                    dis_28,
                    dis_29,
                    dis_34,
                    dis_35,
                    dis_36,
                    dis_37,
                    dis_34,
                    dis_35,
                    dis_36,
                    dis_37,
                };
                int n = dis_data_4x4.size();

                // Step 1: 计算均值
                int sum = std::accumulate(dis_data_4x4.begin(), dis_data_4x4.end(), 0.0);
                double dis_mean_4x4 = static_cast<double>(sum) / n;//单位um
                tof_sense_data.dis_mean = dis_mean_4x4/(1000.0*1000);//单位m
                // Step 1: 计算方差
                double dis_variance_4x4 = std::accumulate(dis_data_4x4.begin(), dis_data_4x4.end(), 0.0,
                                                  [dis_mean_4x4](double accumulator, double x)
                                                  {
                                                      return accumulator + (x - dis_mean_4x4) * (x - dis_mean_4x4);
                                                  }) /n;//单位um
                tof_sense_data.dis_variance = dis_variance_4x4/(1000000.0*1000000);//单位m
                ROS_INFO_STREAM("方差"<<dis_variance_4x4);
                tof_data_pub.publish(tof_sense_data);
                ROS_INFO_STREAM("Published successfully");
            }
            else
            {
                ROS_INFO_STREAM("包头解析错误");
                
            }

            loop_rate.sleep();
        }
    }
    ser2.close();
    return 0;
}